#include <iostream>
#include <vector>
#include <string>
#include <fstream>

#include <cv.h>
#include <highgui.h>

#include "../Common/Utils/utils.h"
#include "../Common/Camera/PGRCamera.h"
#include "../Common/Frameworks/WarpFramework.h"

const int WIDTH = 640;
const int HEIGHT = (WIDTH * 3) / 4;

const int CAMERA_COUNT = 1;
windage::WarpFramework* warp[CAMERA_COUNT];

double INTRINSIC_VALUES[8];
const char* INTRINSIC_FILE_NAME[] = {"intrinsic_1.txt", "intrinsic_2.txt"};
const char* EXTRINSIC_FILE_NAME[] = {"extrinsic_1.txt", "extrinsic_2.txt"};

int selectedCamera = -1;
int selectedPoint = -1;
windage::Vector2 downPoint;
windage::Vector2 beforePoint;
void mouseCallback(int event, int x, int y, int flags, void* param)
{
	windage::Vector2 point(x, y);
	switch(event)
	{
	case CV_EVENT_LBUTTONDOWN:
		{
			downPoint = point;
			double distanceList[CAMERA_COUNT];
			int pointIndex[CAMERA_COUNT];
			for(int i=0; i<CAMERA_COUNT; i++)
			{
				pointIndex[i] = warp[i]->GetNearestPoint(point, &(distanceList[i]));
			}

			int nearestCameraIndex = -1;
			double nearestDistance = 1.0e10;
			for(int i=0; i<CAMERA_COUNT; i++)
			{
				if(nearestDistance > distanceList[i])
				{
					nearestCameraIndex = i;
					nearestDistance = distanceList[i];
				}
			}

			selectedCamera = nearestCameraIndex;
			selectedPoint = pointIndex[nearestCameraIndex];
		}
		break;
	case CV_EVENT_LBUTTONUP:
		selectedCamera = -1;
		selectedPoint = -1;
		break;
	case CV_EVENT_MOUSEMOVE:
		if(selectedCamera >= 0 && selectedPoint >= 0)
		{
			switch(flags)
			{
			case CV_EVENT_FLAG_LBUTTON+CV_EVENT_FLAG_CTRLKEY:	// rotation
				{
					double dy = (point.y - beforePoint.y) * CV_PI/180;
					std::cout << "rotation change : " << dy << std::endl;
					warp[selectedCamera]->ChangeRotation(dy);
				}
				break;
			case CV_EVENT_FLAG_LBUTTON+CV_EVENT_FLAG_ALTKEY:	// translation
				{
					windage::Vector2 deltaT = point - beforePoint;
					std::cout << "translation change : " << deltaT.x << ", " << deltaT.y << std::endl;
					warp[selectedCamera]->ChangeTranslation(deltaT);
				}
				break;
			case CV_EVENT_FLAG_LBUTTON+CV_EVENT_FLAG_SHIFTKEY:// scaling
				{
					double dy = (beforePoint.y - point.y) / (double)HEIGHT;
					dy += 1.0;
					std::cout << "scaling change : " << dy << std::endl;
					warp[selectedCamera]->ChangeScaling(dy);
				}
				break;
			default:
				{
					std::cout << "point change to " << point.x << ", " << point.y << std::endl;
					warp[selectedCamera]->SetAdjustmentPoint(selectedPoint, point);
				}
				break;
			}
		}
		beforePoint = point;
		break;
	break;
	}
}

void main()
{
	cvNamedWindow("warpping window");
	cvSetMouseCallback("warpping window", mouseCallback);

	// load intrinsic parameter for undistortion & extrinsic parameter (homography)
	int width, height;
	windage::Matrix3 h[CAMERA_COUNT];

	for(int i=0; i<CAMERA_COUNT; i++)
	{
		warp[i] = new windage::WarpFramework(WIDTH, HEIGHT);
	
		std::ifstream in(INTRINSIC_FILE_NAME[i]);
		in >> width >> height;
		// intirisic parameters
		in >> INTRINSIC_VALUES[0] >> INTRINSIC_VALUES[1] >> INTRINSIC_VALUES[2] >> INTRINSIC_VALUES[3];
		// distortino cooefficient
		in >> INTRINSIC_VALUES[4] >> INTRINSIC_VALUES[5] >> INTRINSIC_VALUES[6] >> INTRINSIC_VALUES[7];
		in.close();
		
		warp[i]->Initialize(INTRINSIC_VALUES[0], INTRINSIC_VALUES[1], INTRINSIC_VALUES[2], INTRINSIC_VALUES[3],
							INTRINSIC_VALUES[4], INTRINSIC_VALUES[5], INTRINSIC_VALUES[6], INTRINSIC_VALUES[7]);

		// initial extrinsic is 3x3 indentity matrix
		std::ifstream ex(EXTRINSIC_FILE_NAME[i]);
		ex >> h[i].m1[0] >>  h[i].m1[1] >>  h[i].m1[2];
		ex >> h[i].m1[3] >>  h[i].m1[4] >>  h[i].m1[5];
		ex >> h[i].m1[6] >>  h[i].m1[7] >>  h[i].m1[8];
		ex.close();

		warp[i]->SetHomography(h[i]);
	}
	
	
	// result image to merge images
	IplImage* resultImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);

	// camera connection
	CPGRCamera* camera[CAMERA_COUNT];
	IplImage* resizeImage[CAMERA_COUNT];
	IplImage* tempImage[CAMERA_COUNT];
	IplImage* localResultImage[CAMERA_COUNT];
	for(int i=0; i<CAMERA_COUNT; i++)
	{
		resizeImage[i] = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
		tempImage[i] = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
		localResultImage[i] = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);

		camera[i] = new CPGRCamera();
		camera[i]->open();
		camera[i]->start();
	}
	
	bool display[] = {true, true};
	bool updating = false;
	bool processing = true;
	char message[100];
	while(processing)
	{
		double startTime = (double)cvGetTickCount();

		for(int i=0; i<CAMERA_COUNT; i++)
		{
			camera[i]->update();
			IplImage* inputImage = camera[i]->GetIPLImage();
			if(display[i])
				cvResize(inputImage, resizeImage[i]);
			else
				cvZero(resizeImage[i]);

			warp[i]->WarpImageFromHomography(resizeImage[i], tempImage[i]);

			cvCvtColor(tempImage[i], localResultImage[i], CV_GRAY2BGR);
			cvScale(localResultImage[i], localResultImage[i], 1.0/(double)CAMERA_COUNT);
		}

		// merge warped images
		cvSetZero(resultImage);
		for(int i=0; i<CAMERA_COUNT; i++)
		{
			cvAdd(localResultImage[i], resultImage, resultImage);
		}

		// draw select point
		if(selectedCamera >= 0 && selectedPoint >= 0)
		{
			windage::Vector2 point = warp[selectedCamera]->GetAdjustmentPoint(selectedPoint);
			cvCircle(resultImage, cvPoint((int)point.x, (int)point.y), 10, CV_RGB(255, 255, 0), CV_FILLED);
		}

		// draw control points;
		for(int i=0; i<CAMERA_COUNT; i++)
		{
			for(int j=0; j<4; j++)
			{
				int j2 = j==3?0:j+1;
				windage::Vector2 point1 = warp[i]->GetAdjustmentPoint(j);
				windage::Vector2 point2 = warp[i]->GetAdjustmentPoint(j2);

				double r = 0.0;
				double g = 255.0 * ((double)i/(double)(CAMERA_COUNT-1));
				double b = 255.0 * ((double)(CAMERA_COUNT-i)/(double)(CAMERA_COUNT));

				cvCircle(resultImage, cvPoint((int)point1.x, (int)point1.y), 5, CV_RGB(r, g, b), 2);
				cvLine(resultImage, cvPoint((int)point1.x, (int)point1.y), cvPoint((int)point2.x, (int)point2.y), CV_RGB(r, g, b));
			}
		}
		
		double endTime = (double)cvGetTickCount();
		double processingTime = (endTime - startTime) / (cvGetTickFrequency() * 1000.0);
		
		sprintf_s(message, "process time : %.2lf ms", processingTime);
		DrawTextToImage(resultImage, cvPoint(10, 15), message);
		sprintf_s(message, "press 'F' to find chessboard and update pose");
		DrawTextToImage(resultImage, cvPoint(10, 30), message);
		sprintf_s(message, "press 'U' to update pose from control points");
		DrawTextToImage(resultImage, cvPoint(10, 45), message);
		sprintf_s(message, "press 'S' to save current pose");
		DrawTextToImage(resultImage, cvPoint(10, 60), message);
		sprintf_s(message, "mouse 'Left button' to tweak control points");
		DrawTextToImage(resultImage, cvPoint(10, 75), message);
		sprintf_s(message, "mouse 'Left button' with 'Shift' to change scale");
		DrawTextToImage(resultImage, cvPoint(10, 90), message);
		sprintf_s(message, "mouse 'Left button' with 'arT' to change rotation");
		DrawTextToImage(resultImage, cvPoint(10, 105), message);
		sprintf_s(message, "mouse 'Left button' with 'ctRl' to change translation");
		DrawTextToImage(resultImage, cvPoint(10, 120), message);

		cvShowImage("warpping window", resultImage);
		char ch = cvWaitKey(1);
		switch(ch)
		{
		case '1':
			display[0] = !display[0];
			break;
		case '2':
			display[1] = !display[1];
			break;
		case 27:
		case 'q':
		case 'Q':
			processing = false;
			break;
		case 'r':
		case 'R':
			{
				std::cout << "reset adjustment points" << std::endl;
				for(int i=0; i<CAMERA_COUNT; i++)
					warp[i]->ResetAdjustmentPoints();
			}
			break;
		case 'u':
		case 'U':
			{
				std::cout << "update homography from points" << std::endl;
				for(int i=0; i<CAMERA_COUNT; i++)
				{
					warp[i]->UpdateHomographyFromPoints();
					h[i] = warp[i]->GetHomography();
				}
			}
			break;
		case 'f':
		case 'F':
			{
				std::cout << "update homography from chessboard image" << std::endl;
				for(int i=0; i<CAMERA_COUNT; i++)
				{
					warp[i]->UpdateHomographyFromChessboard(resizeImage[i]);
					h[i] = warp[i]->GetHomography();
				}
			}
			break;
		case 's':
		case 'S':
			{
				std::cout << "save homographys" << std::endl;
				for(int i=0; i<CAMERA_COUNT; i++)
				{
					std::ofstream ex(EXTRINSIC_FILE_NAME[i]);
					ex << h[i].m1[0] << " " <<  h[i].m1[1] << " " <<  h[i].m1[2] << std::endl;
					ex << h[i].m1[3] << " " <<  h[i].m1[4] << " " <<  h[i].m1[5] << std::endl;
					ex << h[i].m1[6] << " " <<  h[i].m1[7] << " " <<  h[i].m1[8] << std::endl;
					ex.close();
				}
			}
			break;
		}
	}

	for(int i=0; i<CAMERA_COUNT; i++)
	{
		camera[i]->stop();
		camera[i]->close();
		delete camera[i];
	}

	cvDestroyAllWindows();
}
